program ptc_geometry
use madx_ptc_module
use pointer_lattice
use c_TPSA
implicit none

character*48 :: command_gino
type(probe) xs0,xs1,XST
type(probe_8) xs
type(layout), pointer :: als
real(dp) closed(6),XN(LNV)
type(damap) id,one_turn,a
type(normalform) nf
INTEGER MF,mfc,I,N,k,pos
TYPE(FIBRE),POINTER:: P
type(internal_state) state
type(damapspin) id_s,a_t,a_s,a_f,a_l,a_nl,saved_map
type(normal_spin) nf_S
TYPE(spinor_8) N_AXIS
type(complextaylor) ctt
real(dp) ag,ag0,gamma0,scale 
integer number_of_ac_plane
real(dp) x(6),circ,xt(lnv),prec,theta0 
complex(dp) stune
type(c_taylor) tune
logical(lp) mis
type(work) E_0 
real(dp) mat(6,6),vv(6),aa,xnn
type(real_8) theta,N0(3)  
!-----------------------------------

type(c_damap) c_map,c_spin0,as
type(c_normal_form) c_n
TYPE(c_spinor) C_N0,C_N1
type(c_taylor) s(6),inv,ct
 
    interface
       subroutine build_ALS(ALS,MIS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: ALS
         logical(lp) mis
       end subroutine build_ALS
    end interface


Lmax = 10.d0
use_info = .true.
 
prec=1.d-10 !

call ptc_ini

ALS=>m_u%start

mis=my_true  ! big misalignment is put

call build_ALS(ALS,mis) 

 
m_U%start%name="ALS "
write(6,*) "Making an node layout"
CALL MAKE_NODE_LAYOUT(m_U%start)

als=>m_U%start
p=>als%start


!!!! circ is the circumference of the ring !!!! 
call get_length(als,circ)

!!!! modulate.txt sets the magnet BEND1 as a modulated magnet !!!! 
 call kanalnummer(mf,file="AC_modulation.txt")
  write(mf,*) "select layout"
  write(mf,*) 1
  write(mf,*) " MODULATE"
  write(mf,*) " BEND1" 
  write(mf,*) "1.d0 1.d-3 0.00d0   !   DC_ac,A_ac,theta_ac"
  write(mf,*) "1.d0   1      ! 0  !D_ac,n_ac, n_coeff "
  write(mf,*) "1 0 0.000003d0 "
  write(mf,*) "0  0 0 "
  write(mf,*) " return "
 close(mf)
 call read_ptc_command77("AC_modulation.txt")

!

ag0=  1.158630183290000D-003*1.01
p=>als%start
E_0=p
 
!!!!!  Set all magnets spin to g=ag0 to fake a resonance!!!!!! 
p=>als%start
do i=1,als%n
 p%ag = ag0  
 IF(P%MAG%P%NMUL>=3) THEN
  CALL ADD(P,3,0,0.D0) ! making the lattice linear (not really needed)
 ENDIF
 p=>p%next
enddo
 
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

closed=0.d0
state=default0+nocavity0  

    CALL FIND_ORBIT(ALS,CLOSED,1,STATE,c_1d_5)
 
state=default0+only_4d0+SPIN0
XS0=CLOSED
    CALL FIND_ORBIT_probe_spin(ALS,XS0,STATE,c_1d_5,FIBRE1=1,theta0=theta0)


state=default0+only_4d0+SPIN0+modulation0

!!!! set a modulation clock !!!!!!
xs0%ac%om=0.401073997029287d0*twopi/circ
xs0%ac%x(1)=1.d0
xs0%ac%x(2)=0.d0
write(6,*) " Modulation tune =",circ*xs0%ac%om/twopi

!!!! xs0%ac%x(1) and xs0%ac%x(2) will be the (q,p) of the modulating clock
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 call kanalnummer(mfc,file="spin_invariant.txt")
    CALL INIT(STATE,4,0)

    call alloc(xs)
    call alloc(N_AXIS)    
    call alloc(id_S)
    call alloc(a_t,a_s,a_f,a_l,a_nl,saved_map)
    call alloc(nf_S)

!!!! Polymorphic probe is created in the usual manner   
   ID_S=1
   
   XS=XS0+ID_S     !
   
!!!! get spin polymorphic probe after one turn   
CALL TRACK_PROBE(ALS,XS,STATE,FIBRE1=1)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 

!!!! Copy probe_8 into a damapspin
ID_S=XS
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 

saved_map=id_s

!!! Map normalised
NF_S=ID_S  ! Normalisation

a_t=NF_S%a_t
!! (a_t%m,a_t%s) = (I ,a_s%s) o (a_f%m, I ) o (a_l%m,I) o (a_nl%m,I)
call factor(a_t,a_s,a_f,a_l,a_nl)    
call fetch_s0(a_s,a_s)
ID_S=a_s**(-1)*ID_S*a_s  ! here a_s is only A_0

NF_S=ID_S ! Normalisation

a_t=NF_S%a_t
!! (a_t%m,a_t%s) = (I ,a_s%s) o (a_f%m, I ) o (a_l%m,I) o (a_nl%m,I)
call factor(a_t,a_s,a_f,a_l,a_nl)   
 
n_axis=2  !  Creating e_y=(0,1,0)

n_axis=a_s*n_axis ! ISF created


 
number_of_ac_plane=2
call c_init(c_%NO,c_%nd+1,c_%np,c_%ndpt,number_of_ac_plane,ptc=my_true)  

call alloc(c_map)
call alloc(as)
call alloc(c_n)
call alloc(c_spin0)
CALL ALLOC(C_N0)
call alloc(inv)
call alloc(ct)
call alloc(ctt)

c_map=1  ! because of extra excess spin dimensions 
c_map=saved_map

C_SPIN0=0
call c_normal_spin_linear(c_map,C_SPIN0,as,c_n0)

call c_convert_spin(C_SPIN0,C_SPIN0)

call c_normal(C_SPIN0,c_n,dospin=my_false)

write(mfc,'(a9,4(1x,G21.14))') " tunes = ",c_n%tune(1:4)

! Computing the tune with the real FPP normal form
! which uses spectator spin
!

write(mfc,*) " "
write(mfc,*) "Leading order amplitude dependent spin tune"
write(mfc,*) "using the spectator normal form"
 
call alloc(n0)
call alloc(theta)
 
ID_S=NF_S%a_t**(-1)*ID_S*NF_S%a_t
call get_spin_n0(ID_S,theta,n0)
theta=(theta.cut.3)-(theta.cut.1)
call print(theta,mfc,prec)

write(mfc,*) " "
write(mfc,*) "Same terms computed with the extended phase space"
 
call print(c_n%ker%f(3)%v(8),mfc,prec)


c_n0%v(1)=(1.0_dp.cmono.1)**2+(1.0_dp.cmono.2)**2
c_n0%v(2)=(1.0_dp.cmono.3)**2+(1.0_dp.cmono.4)**2
c_n0%v(3)=(1.0_dp.cmono.7)**2+(1.0_dp.cmono.8)**2


do i=1,3
 c_n0%v(i)=c_n0%v(i)*c_n%a_t**(-1)
enddo
 
     do i=1,6
      call alloc(s(i))
     enddo
  
     s(1)=(1.0_dp.cmono.7)
     s(3)=(1.0_dp.cmono.8)
     s(2)= sqrt(1.0_dp-s(1)**2-s(3)**2)

inv=0.0_dp
do i=1,3
 ctt=N_AXIS%x(i)%t
 ct=ctt
 inv=ct*s(i)+inv
enddo


inv=1.0_dp-inv**2
inv=inv-(inv.sub.'0')

write(mfc,*) " "
write(mfc,*) " 1.0_dp-n.s**2  computed using the usual spectator normal form"
call print(inv,mfc,prec)

write(6,'(a10,(1x,G21.14),a2)') " norm 0 = ", 100*full_abs(c_n0%v(3)-inv)/full_abs(inv)," %"
write(6,*) " "
s(1)=c_n0%v(1)
s(2)=c_n0%v(2)
s(3)=c_n0%v(3)
mat=0.d0
vv=0.d0

do i=1,3
  mat(1,i)= s(i).sub.'200'
  mat(2,i)= s(i).sub.'1010'
  mat(3,i)= s(i).sub.'002'
enddo
  vv(1)=(inv.sub.'200')- (c_n0%v(3).sub.'200') 
  vv(2)=(inv.sub.'1010')- (c_n0%v(3).sub.'1010') 
  vv(3)=(inv.sub.'002')- (c_n0%v(3).sub.'002') 

    call matinv(mat,mat,3,6,i)

    vv=matmul(mat,vv)

 write(6,*) " Proportions of other invariants  "   
 write(6,'(3(1x,G21.14))') vv(1:3)
 write(6,'(3(1x,G21.14))') (vv(i)/full_abs(s(i)),i=1,3)
 write(6,*) "   "

   do i=1,3
    c_n0%v(3)=c_n0%v(3)+vv(i)*s(i)
   enddo


write(mfc,*) " final invariant 1  "
call print(c_n0%v(3),mfc,prec)


write(6,'(a10,(1x,G21.14),a2)') " norm 1 = ", 100*full_abs(c_n0%v(3)-inv)/full_abs(inv),"%"
write(6,*) " "

s(1)=c_n0%v(3)**2
s(2)=c_n0%v(1)**2
s(3)=c_n0%v(2)**2
s(4)=c_n0%v(1)*c_n0%v(2)
s(5)=c_n0%v(1)*c_n0%v(3)
s(6)=c_n0%v(2)*c_n0%v(3)


mat=0.d0
vv=0.d0

do i=1,6
  mat(1,i)= s(i).sub.'400'
  mat(2,i)= s(i).sub.'1111'
  mat(3,i)= s(i).sub.'004'
  mat(4,i)= s(i).sub.'0022'
  mat(5,i)= s(i).sub.'0202'
  mat(6,i)= s(i).sub.'2002'
enddo
  vv(1)=(inv.sub.'400')- (c_n0%v(3).sub.'400') 
  vv(2)=(inv.sub.'1111')- (c_n0%v(3).sub.'1111') 
  vv(3)=(inv.sub.'004')- (c_n0%v(3).sub.'004') 
  vv(4)=(inv.sub.'0022')- (c_n0%v(3).sub.'0022') 
  vv(5)=(inv.sub.'0202')- (c_n0%v(3).sub.'0202') 
  vv(6)=(inv.sub.'2002')- (c_n0%v(3).sub.'2002') 


    call matinv(mat,mat,6,6,i)

    vv=matmul(mat,vv)
 

 write(6,*) " Proportions of other invariants  "
 write(6,'(6(1x,G21.14))') vv
 write(6,'(6(1x,G21.14))') (vv(i)/full_abs(s(i)),i=1,6)
  write(6,*) "    "

   do i=1,6
    c_n0%v(3)=c_n0%v(3)+vv(i)*s(i)
   enddo

write(6,'(a10,(1x,G21.14),a2)') " norm 2 = ", 100*full_abs(c_n0%v(3)-inv)/full_abs(inv),"%"
write(6,*) " "

write(mfc,*) " final invariant 2  "
call print(c_n0%v(3),mfc,prec)


  
 close(mfc)


call kill(c_map)
call kill(as)
call kill(c_n)
call kill(c_spin0)
CALL kill(C_N0)
call kill(inv)
call kill(ct)
call kill(ctt)
call kill(xs)
call kill(N_AXIS)    
call kill(id_S)
call kill(a_t,a_s,a_f,a_l,a_nl,saved_map)
call kill(nf_S)
call kill(n0)
call kill(theta)
 

 call ptc_end


end program ptc_geometry

subroutine  build_ALS(ALS,mis)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: ALS

real(dp) :: alpha,lbend, cut, ksd, ksf 
type(fibre)  L1,L2,L3,L4,L5,L6,L7,L8,L9,L10
type(fibre)  L11,L12,L13,L14,L15,L16,L17,L18,L19,L20
type(fibre)  L21,L22,L23,L24,L25,L26,L27,L27A,L27B,L27C,L27D,DS
 type(fibre)  QF1,QF2,QD1,QD2,QFA1,QFA2,sf,sd,cav,bend,vc5,bend1
type(layout) :: sfline,sdline,sup1,supb
logical(lp) mis
!-----------------------------------

call make_states(.true.)
exact_model = .false.
!default = default + nocavity  
call update_states
madlength = .false.


call set_mad(energy = 1.5d0, method = 6, step = 3)

madkind2 = drift_kick_drift


  L1  = drift("L1 ",  2.832695d0)
  L2  = drift("L2 ",  0.45698d0)
  L3  = drift("L3 ",  0.08902d0)
  L4  = drift("L4 ",  0.2155d0)
  L5  = drift("L5 ",  0.219d0)
  L6  = drift("L6 ",  0.107078d0)
  L7  = drift("L7 ",  0.105716d0)
  L8  = drift("L8 ",  0.135904d0)
  L9  = drift("L9 ",  0.2156993d0)
  L10 = drift("L10",  0.089084d0)
   L11= drift("L11",  0.235416d0)
   L12= drift("L12",  0.1245d0)
   L13= drift("L13",  0.511844d0)
   L14= drift("L14",  0.1788541d0)
   L15= drift("L15",  0.1788483d0)
   L16= drift("L16",  0.511849d0)
   L17= drift("L17",  0.1245d0)
   L18= drift("L18",  0.235405d0)
   L19= drift("L19",  0.089095d0)
   L20= drift("L20",  0.2157007d0)
   L21= drift("L21",  0.177716d0)
   L22= drift("L22",  0.170981d0)
   L23= drift("L23",  0.218997d0)
 L24 = drift ("L24",  0.215503d0)
 L25 = drift ("L25",  0.0890187d0)
 L26 = drift ("L26",  0.45698d0)
 L27 = drift ("L27",  2.832696d0)
 L27a  = drift (" L27a",  0.8596d0)
 L27b  = drift (" L27b",  0.1524d0)
 L27c  = drift (" L27c",  0.04445d0)
 L27d  = drift (" L27d",  1.776246d0)
 ds  = drift (" DS  ", 0.1015d0)

  QF1 = QUADRUPOLE(" QF1 ",0.344D0, K1= 2.2474D0+6.447435260914397D-03)
  QF2 = QUADRUPOLE(" QF2 ",0.344D0, K1= 2.2474D0)
  QD1 = QUADRUPOLE(" QD1 ",0.187D0, K1= -2.3368D0-2.593018157427161D-02); 
  QD2 = QUADRUPOLE(" QD2 ",0.187D0, K1= -2.3368D0);  
  QFA1= QUADRUPOLE(" QFA1",0.448D0, K1= 2.8856D0);  
  QFA2= QUADRUPOLE(" QFA2",0.448D0, K1= 2.8856D0);  

!!! 1/2 mad-x value
ksf=(-41.67478927130080d0+0.3392376315938252d0);ksd= (56.36083889436033d0-0.1043679358857811d0);
   sf=sextupole ("sf",2.d0*0.1015d0, K2= ksf);
   sd= sextupole("sd", 2.d0*0.1015d0, K2= ksd);

 VC5=marker("vc5");
ALPHA=0.17453292519943295769236907684886d0;
 
LBEND=0.86621d0;
 
 
BEND = RBEND("BEND", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
BEND1 = RBEND("BEND1", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
 
CAV=RFCAVITY("CAV",L=0.0000d0,VOLT=-1.0d0,REV_FREQ=500.0d6)


sfline=1*sf;
sdline=1*sd;

SUP1=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

SUPb=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND1+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

ALS = 11*sup1+supb+cav;
 
ALS = .ring.ALS

call survey(ALS)


if(mis) then
 sig=1.d-5
 cut=4.d0
 call MESS_UP_ALIGNMENT(ALS,SIG,cut)
endif
end subroutine build_ALS